from Astar import Astar
import json
import time
import numpy as np
import os
#import route_fy_v2.envs.drawmap as dm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

with open('D:\git\mytools\pathdata.json', 'r') as f:
    data = json.load(f)


map=np.array(data['Grid_Map'])

#print(map.shape)
s=data['PathStartPoint'][0]
e=data['PathEndPoint'][0]
#print(s,e)
#利用time计时
start=time.time()
A=Astar(map=np.array(map),start=s,end=e)
result,info=A.FindPath()
end=time.time()
print("time:",end-start)
print()
#print(result,info)
father=False
centerlist=[]
openlist=[]
closelist=[]
while father==False:
    centerlist.append(info.position.tolist())
    info=info.prepoint
    father=(info.prepoint is None)
centerlist.append(info.position.tolist())
for i in A.open_set:
    openlist.append(i.position)
    
for i in A.close_set:
    closelist.append(i.position)
result_map=map
    
for i in openlist:
    result_map[i[0],i[1],i[2]]=3
for i in closelist:
    result_map[i[0],i[1],i[2]]=3
for i in centerlist:
    result_map[i[0],i[1],i[2]]=2
#cm.drawmap(result_map)
#p=dm.P_show()
#p.draw2(result_map)
#p.result()

# 假设你的三维张量是一个numpy数组
tensor = map

fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# 获取张量的坐标和值
x, y, z = np.where(tensor == 1)
l, m, n = np.where(tensor == 2)
# 在对应的位置上画点
#ax.scatter(x, y, z, c='b', marker='o', alpha=0.1)

# 使用 plot 函数绘制3D线条
ax.plot(l, m, n, color='r', alpha=0.5)

ax.scatter(l, m, n, c='r', marker='o', alpha=0.5)
#ax.scatter(e_array, f_array, g_array, c='c', marker='o', alpha=0.5)
ax.scatter(0, 0, 0 , c='b', marker='o', alpha=0.5)
ax.scatter(s[0], s[1], s[2] , c='y', marker='o', alpha=1)
ax.scatter(e[0], e[1], e[2] , c='g', marker='o', alpha=1)
ax.scatter(tensor.shape[0], tensor.shape[1], tensor.shape[2] , c='b', marker='o', alpha=0.5)
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')

plt.show()

data_dict = {
    "centerlist": centerlist
}
json_data = json.dumps(data_dict)
# 保存到文件

f_path = os.path.join( "D:\ARouting.json")
f_path=os.path.normpath(f_path).replace("\\", "/")
with open(f_path, 'w') as f:
    json.dump(data_dict, f)
print(f_path)